#include "DecoSolidObject.h"
#include "RenderInterface.h"
#include "LzmaDecode.h"
#include "ConfigManager.h"
#include "GridField.h"
#include "DecoLogger.h"
//#include "slcSurface.H"

DecoSolidObject::DecoSolidObject(void)
:	DecoSceneObject(UndefinedT),
	m_rho(1),
	m_mass(0),
	m_inertia(matrix33(0)),
	m_linMomentum(vector3(0, 0, 0)),
	m_angMomentum(vector3(0, 0, 0)),
	m_acc(vector3(0, 0, 0)),
	m_vel(vector3(0, 0, 0)),
	m_omg(vector3(0, 0, 0)),
	m_currentTime(0),
	m_bTwowayCouple(FALSE)
{
	bIsRenderable = TRUE;
	bIsMovable = TRUE;
	bIsSelectable = TRUE;
	bIsRotatable = TRUE;
}

DecoSolidObject::~DecoSolidObject(void)
{
}


void DecoSolidObject::calculateMass()
{
	if (m_bTwowayCouple)
	{
		int numIndices = static_cast<int>(m_associateGridIndex.size());
		double massPerCell = m_rho * pow(m_grid->stepSize, 3);
		m_mass = massPerCell * numIndices;
		int i, j, k;
		m_COM = vector3(0, 0, 0);
		for (int it = 0; it < numIndices; it++)
		{
			i = m_associateGridIndex[it].m_i;
			j = m_associateGridIndex[it].m_j;
			k = m_associateGridIndex[it].m_k;

			m_COM += massPerCell * m_grid->GetGridCenter(i, j, k);
		}
		m_COM /= m_mass;
	}

}

void DecoSolidObject::calculateInertia()
{
	if (m_bTwowayCouple)
	{
		m_inertia = 0;
		double massPerCell = m_rho * pow(m_grid->stepSize, 3);
		int i, j, k;
		int numIndices = static_cast<int>(m_associateGridIndex.size());
		for (int it = 0; it < numIndices; it++)
		{
			i = m_associateGridIndex[it].m_i;
			j = m_associateGridIndex[it].m_j;
			k = m_associateGridIndex[it].m_k;
			vector3 r = m_grid->GetGridCenter(i, j, k) - m_COM;
			matrix33 rOPr = OuterProduct(r, r);
			double rSqr = r.lengthSqr();

			m_inertia += massPerCell * (rSqr * IdentityMatrix33() - rOPr);
		}
	}


}



void DecoSolidObject::UpdateBeforeCoupling(DOUBLE deltaTime, BOOL bSimplifiedFluid)
{
	//m_vel += deltaTime * vector3(0, -9.8, 0);
}

void DecoSolidObject::calculateMomentum()
{
	m_linMomentum = m_mass * m_vel;
	m_angMomentum = m_inertia * m_omg;
}

void DecoSolidObject::updateRigidBody(const vector3& force, const vector3& torque, DOUBLE deltaTime)
{
	vector3 forceWithoutGravity = force;// + m_mass * vector3(0, -9.8, 0);
	//m_linMomentum += deltaTime * forceWithoutGravity;
	//m_vel = m_linMomentum / m_mass;
	m_vel += deltaTime * forceWithoutGravity / m_mass;
	vector3 translateOffset = m_vel * deltaTime;
	coords.Translate(translateOffset);

	//m_angMomentum += deltaTime * torque;
	//m_omg = m_inertia.invert() * m_angMomentum;
	m_omg += deltaTime * m_inertia.invert() * torque;
	double angle = m_omg.length() * deltaTime;
	if (abs(angle) > EPSILON_FLOAT)
	{
		vector3 dir = m_omg;
		dir.normalize();
		coords.RotateAlongGlobalVector(angle, dir);
	}
	(*DecoLogger::GetSingleton()) << "Force  :" << forceWithoutGravity << "\n";
	(*DecoLogger::GetSingleton()) << "Torque :" << torque << "\n";
}


FluidSparseVector DecoSolidObject::getPressureContributionToUFaceAccForRigidBody(int i, int j, int k, int dim)
{
	FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
	vector3 dirFromOrigin;
	if (m_grid->isFluidCell(i - 1, j, k) && m_grid->isSolidCell(i, j, k))
	{
		vector3 normal(1, 0, 0);
		vector3 posGlobal = m_grid->GetGridCenter(i - 1, j, k) + vector3(m_grid->stepSize / 2, 0, 0);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.x * m_ForceX + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;
	}
	else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i - 1, j, k))
	{
		vector3 normal(-1, 0, 0);
		vector3 posGlobal = m_grid->GetGridCenter(i, j, k) + vector3(-m_grid->stepSize / 2, 0, 0);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.x * m_ForceX + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;

	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToUFaceAcc(" << i << ", " << j << ", " << k <<").\n";
	}
	return ret;
}
FluidSparseVector DecoSolidObject::getPressureContributionToVFaceAccForRigidBody(int i, int j, int k, int dim)
{
	FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
	vector3 dirFromOrigin;
	if (m_grid->isFluidCell(i, j - 1, k) && m_grid->isSolidCell(i, j, k))
	{
		vector3 normal(0, 1, 0);
		vector3 posGlobal = m_grid->GetGridCenter(i, j - 1, k) + vector3(0, m_grid->stepSize / 2, 0);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.y * m_ForceY + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;
	}
	else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i, j - 1, k))
	{
		vector3 normal(0, -1, 0);
		vector3 posGlobal = m_grid->GetGridCenter(i, j, k) + vector3(0, -m_grid->stepSize / 2, 0);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.y * m_ForceY + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;

	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToVFaceAcc(" << i << ", " << j << ", " << k <<").\n";
	}
	return ret;
}
FluidSparseVector DecoSolidObject::getPressureContributionToWFaceAccForRigidBody(int i, int j, int k, int dim)
{
	FluidSparseVector ret(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
	vector3 dirFromOrigin;
	if (m_grid->isFluidCell(i, j, k - 1) && m_grid->isSolidCell(i, j, k))
	{
		vector3 normal(0, 0, 1);
		vector3 posGlobal = m_grid->GetGridCenter(i, j, k - 1) + vector3(0, 0, m_grid->stepSize / 2);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.z * m_ForceZ + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;
	}
	else if (m_grid->isFluidCell(i, j, k) && m_grid->isSolidCell(i, j, k - 1))
	{
		vector3 normal(0, 0, -1);
		vector3 posGlobal = m_grid->GetGridCenter(i, j, k) + vector3(0, 0, -m_grid->stepSize / 2);
		dirFromOrigin = posGlobal - m_COM;
		dirFromOrigin.normalize();
		vector3 rCrossN = CrossProduct(dirFromOrigin, normal);
		ret = normal.z * m_ForceZ + rCrossN.x * m_TorqueX + rCrossN.y * m_TorqueY + rCrossN.z * m_TorqueZ;

	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here! GetPressureContributionToWFaceAcc(" << i << ", " << j << ", " << k <<").\n";
	}
	return ret;
}

void DecoSolidObject::prepareCouplingMatrixForRigidBody(int dim)
{
	if (m_bTwowayCouple)
	{
		m_ForceX.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_ForceY.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_ForceZ.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_TorqueX.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_TorqueY.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_TorqueZ.SetDimension(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		m_ForceX.SetZero();
		m_ForceY.SetZero();
		m_ForceZ.SetZero();
		m_TorqueX.SetZero();
		m_TorqueY.SetZero();
		m_TorqueZ.SetZero();


		int numAssociatedCells = static_cast<int>(m_associateGridIndex.size());
		double area = m_grid->stepSize * m_grid->stepSize;
		vector3 force, dirFromOrigin, torque;
		for (int it = 0; it < numAssociatedCells; ++it)
		{
			int i = m_associateGridIndex[it].m_i;
			int j = m_associateGridIndex[it].m_j;
			int k = m_associateGridIndex[it].m_k;
			if (m_grid->isFluidCell(i + 1, j, k))
			{
				vector3 normal = vector3(-1, 0, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i + 1, j, k) + vector3(-m_grid->stepSize / 2, 0, 0);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceX.SetValue(i + 1, j, k, m_ForceX.GetValue(i + 1, j, k) - area);

				m_TorqueX.SetValue(i + 1, j, k, m_TorqueX.GetValue(i + 1, j, k) + torque.x);
				m_TorqueY.SetValue(i + 1, j, k, m_TorqueY.GetValue(i + 1, j, k) + torque.y);
				m_TorqueZ.SetValue(i + 1, j, k, m_TorqueZ.GetValue(i + 1, j, k) + torque.z);
			}
			if (m_grid->isFluidCell(i - 1, j, k))
			{
				vector3 normal = vector3(1, 0, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i - 1, j, k) + vector3(m_grid->stepSize / 2, 0, 0);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceX.SetValue(i - 1, j, k, m_ForceX.GetValue(i - 1, j, k) + area);

				m_TorqueX.SetValue(i - 1, j, k, m_TorqueX.GetValue(i - 1, j, k) + torque.x);
				m_TorqueY.SetValue(i - 1, j, k, m_TorqueY.GetValue(i - 1, j, k) + torque.y);
				m_TorqueZ.SetValue(i - 1, j, k, m_TorqueZ.GetValue(i - 1, j, k) + torque.z);

			}
			if (m_grid->isFluidCell(i, j + 1, k))
			{
				vector3 normal = vector3(0, -1, 0);
				vector3 posGlobal = m_grid->GetGridCenter(i, j + 1, k) + vector3(0, -m_grid->stepSize / 2, 0);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceY.SetValue(i, j + 1, k, m_ForceY.GetValue(i, j + 1, k) - area);

				m_TorqueX.SetValue(i, j + 1, k, m_TorqueX.GetValue(i, j + 1, k) + torque.x);
				m_TorqueY.SetValue(i, j + 1, k, m_TorqueY.GetValue(i, j + 1, k) + torque.y);
				m_TorqueZ.SetValue(i, j + 1, k, m_TorqueZ.GetValue(i, j + 1, k) + torque.z);

			}
			if (m_grid->isFluidCell(i, j - 1, k))
			{
				vector3 normal = vector3(0, 1, 0);

				vector3 posGlobal = m_grid->GetGridCenter(i, j - 1, k) + vector3(0, m_grid->stepSize / 2, 0);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceY.SetValue(i, j - 1, k, m_ForceY.GetValue(i, j - 1, k) + area);

				m_TorqueX.SetValue(i, j - 1, k, m_TorqueX.GetValue(i, j - 1, k) + torque.x);
				m_TorqueY.SetValue(i, j - 1, k, m_TorqueY.GetValue(i, j - 1, k) + torque.y);
				m_TorqueZ.SetValue(i, j - 1, k, m_TorqueZ.GetValue(i, j - 1, k) + torque.z);

			}
			if (m_grid->isFluidCell(i, j, k + 1))
			{
				vector3 normal = vector3(0, 0, -1);

				vector3 posGlobal = m_grid->GetGridCenter(i, j, k + 1) + vector3(0, 0, -m_grid->stepSize / 2);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceZ.SetValue(i, j, k + 1, m_ForceZ.GetValue(i, j, k + 1) - area);

				m_TorqueX.SetValue(i, j, k + 1, m_TorqueX.GetValue(i, j, k + 1) + torque.x);
				m_TorqueY.SetValue(i, j, k + 1, m_TorqueY.GetValue(i, j, k + 1) + torque.y);
				m_TorqueZ.SetValue(i, j, k + 1, m_TorqueZ.GetValue(i, j, k + 1) + torque.z);

			}
			if (m_grid->isFluidCell(i, j, k - 1))
			{
				vector3 normal = vector3(0, 0, 1);

				vector3 posGlobal = m_grid->GetGridCenter(i, j, k - 1) + vector3(0, 0, m_grid->stepSize / 2);
				dirFromOrigin = posGlobal - m_COM;
				dirFromOrigin.normalize();
				torque = CrossProduct(dirFromOrigin, normal) * area;

				m_ForceZ.SetValue(i, j, k - 1, m_ForceZ.GetValue(i, j, k - 1) + area);

				m_TorqueX.SetValue(i, j, k - 1, m_TorqueX.GetValue(i, j, k - 1) + torque.x);
				m_TorqueY.SetValue(i, j, k - 1, m_TorqueY.GetValue(i, j, k - 1) + torque.y);
				m_TorqueZ.SetValue(i, j, k - 1, m_TorqueZ.GetValue(i, j, k - 1) + torque.z);

			}
		}
		m_ForceX /= m_mass;
		m_ForceY /= m_mass;
		m_ForceZ /= m_mass;
		matrix33 invInertia = m_inertia.invert();
		FluidSparseVector angAccX(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		FluidSparseVector angAccY(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		FluidSparseVector angAccZ(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, dim);
		angAccX = invInertia[0][0] * m_TorqueX + invInertia[1][0] * m_TorqueY + invInertia[2][0] * m_TorqueZ;
		angAccY = invInertia[0][1] * m_TorqueX + invInertia[1][1] * m_TorqueY + invInertia[2][1] * m_TorqueZ;
		angAccZ = invInertia[0][2] * m_TorqueX + invInertia[1][2] * m_TorqueY + invInertia[2][2] * m_TorqueZ;
		m_TorqueX = angAccX;
		m_TorqueY = angAccY;
		m_TorqueZ = angAccZ;
	}
}


DecoArchive& operator<< (DecoArchive& Ar, const DecoSolidObject& obj)
{
	Ar << obj.m_vel;
	Ar << obj.m_acc;
	Ar << obj.m_omg;
	Ar << obj.m_currentTime;

	Ar << obj.m_rho;
	Ar << obj.m_mass;
	Ar << obj.m_inertia;
	Ar << obj.m_COM;
	Ar << obj.m_angMomentum;
	Ar << obj.m_linMomentum;
	Ar << obj.m_bTwowayCouple;

	const DecoSceneObject* const scnObj = &obj;
	scnObj->Serialize(Ar);
	return Ar;
}
DecoArchive& operator>> (DecoArchive& Ar, DecoSolidObject& obj)
{
	Ar >> obj.m_vel;
	Ar >> obj.m_acc;
	Ar >> obj.m_omg;
	Ar >> obj.m_currentTime;

	Ar >> obj.m_rho;
	Ar >> obj.m_mass;
	Ar >> obj.m_inertia;
	Ar >> obj.m_COM;
	Ar >> obj.m_angMomentum;
	Ar >> obj.m_linMomentum;
	Ar >> obj.m_bTwowayCouple;

	DecoSceneObject* scnObj = &obj;
	scnObj->Deserialize(Ar);
	return Ar;

}



